/*
 * topological-hex, a program to compute combinatorial hexahedral meshes.
 *
 * Copyright (C) <2018> <Université catholique de Louvain (UCL), Belgique>
 *
 * List of the contributors to the development, description and complete
 * License: see LICENSE file.
 *
 * This program (topological-hex) is free software:
 * you can redistribute it and/or modify it under the terms
 * of the GNU General Public License as published by the Free
 * Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program (see COPYING file).  If not,
 * see <http://www.gnu.org/licenses/>.
 */

#include <stdlib.h>
#include <string.h>

#include "queue.h"

error_code concurrent_queue_init(concurrent_queue *queue, uint32_t num_slots,
                                 uint32_t element_size) {
  error_code status = SUCCESS;

  if (pthread_mutex_init(&queue->lock, NULL) != 0) {
    status = SYSTEM_ERROR;
    goto fail_create_mutex;
  }

  if (pthread_cond_init(&queue->empty_cond, NULL) != 0) {
    status = SYSTEM_ERROR;
    goto fail_create_empty_cond;
  }

  if (pthread_cond_init(&queue->full_cond, NULL) != 0) {
    status = SYSTEM_ERROR;
    goto fail_create_full_cond;
  }

  if (posix_memalign(&queue->data, ALIGNMENT, element_size * num_slots) != 0) {
    status = OUT_OF_MEMORY;
    goto fail_alloc_data;
  }

  queue->closed = false;

  queue->offset = 0;
  queue->size = 0;
  queue->capacity = num_slots;
  queue->element_size = element_size;

  return SUCCESS;

  free(queue->data);
fail_alloc_data:
  pthread_cond_destroy(&queue->full_cond);
fail_create_full_cond:
  pthread_cond_destroy(&queue->empty_cond);
fail_create_empty_cond:
  pthread_mutex_destroy(&queue->lock);
fail_create_mutex:
  return status;
}

void concurrent_queue_release(concurrent_queue *queue) {
  free(queue->data);
  pthread_cond_destroy(&queue->full_cond);
  pthread_cond_destroy(&queue->empty_cond);
  pthread_mutex_destroy(&queue->lock);
}

void concurrent_queue_push(concurrent_queue *queue, const void *data) {
  pthread_mutex_lock(&queue->lock);
  while (queue->size == queue->capacity)
    pthread_cond_wait(&queue->full_cond, &queue->lock);

  uint32_t slot_id = (queue->offset + queue->size) % queue->capacity;
  uint32_t byte_offset = queue->element_size * slot_id;
  memcpy((char*)queue->data + byte_offset, data, queue->element_size);
  queue->size++;

  pthread_mutex_unlock(&queue->lock);

  pthread_cond_signal(&queue->empty_cond);
}

bool concurrent_queue_pop(concurrent_queue *queue, void *data) {
  pthread_mutex_lock(&queue->lock);
  while (queue->size == 0 && !queue->closed)
    pthread_cond_wait(&queue->empty_cond, &queue->lock);

  if (queue->size > 0) {
    uint32_t byte_offset = queue->element_size * queue->offset;
    memcpy(data, (char*)queue->data + byte_offset, queue->element_size);

    queue->offset++;
    if (queue->offset == queue->capacity) queue->offset = 0;
    queue->size--;

    pthread_mutex_unlock(&queue->lock);

    pthread_cond_signal(&queue->full_cond);

    return true;
  }
  else {
    pthread_mutex_unlock(&queue->lock);
    return false;
  }
}

bool concurrent_queue_try_pop(concurrent_queue *queue, void *data) {
  pthread_mutex_lock(&queue->lock);

  if (queue->size > 0) {
    uint32_t byte_offset = queue->element_size * queue->offset;
    memcpy(data, (char*)queue->data + byte_offset, queue->element_size);

    queue->offset++;
    if (queue->offset == queue->capacity) queue->offset = 0;
    queue->size--;

    pthread_mutex_unlock(&queue->lock);

    pthread_cond_signal(&queue->full_cond);

    return true;
  }
  else {
    pthread_mutex_unlock(&queue->lock);
    return false;
  }
}

void concurrent_queue_close(concurrent_queue *queue) {
  pthread_mutex_lock(&queue->lock);
  queue->closed = true;
  pthread_mutex_unlock(&queue->lock);

  pthread_cond_broadcast(&queue->empty_cond);
}
